//*****************************************************************************
// File Name	: MiniOre32LF1.c
//
// Title		: MiniOre32 Line Follower Mark 1
// Revision		: See version.h for versioning info.
// Notes		: The program is makes extensive use of avrlib
// Target MCU	: Atmel ATmega32 series
// Editor Tabs	: 4
//
// Revision History:
// When			Who			Description of change
// -----------	-----------	-----------------------
// 2009.02.17 	andrzej	    Created the program
//*****************************************************************************

//----- Include Files ---------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

#include <avr/io.h>			// include I/O definitions (port names, pin names, etc)
#include <avr/interrupt.h>	// include interrupt support
#include <avr/eeprom.h>
#include <util/delay.h>

#include "global.h"		// include our global settings
#include "uart.h"		// include uart function library
#include "rprintf.h"	// include printf function library
#include "a2d.h"		// include A/D converter function library
#include "timer.h"		// include timer function library (timing, PWM, etc)
#include "vt100.h"		// include vt100 terminal support
#include "cmdline.h"	// include cmdline function library
#include "extint.h"		// include external interrupt library
#include "buffer.h"
#include "MOvember2012.h"

#ifndef CRITICAL_SECTION_START
#define CRITICAL_SECTION_START	unsigned char _sreg = SREG; cli()
#define CRITICAL_SECTION_END	SREG = _sreg
#endif

#define FULLVERSION_STRING "2012.04.17"

#define ERROR_BUFFER_SIZE 128

void GetSensorReading(void);
void TestLoop(void);

// global variables
LED_t Led;
PID_t Pid;
State_t State;
uint8_t debug;
uint8_t mode;
static int16_t error_vector[ERROR_BUFFER_SIZE];
int16Buffer_t ErrorBuffer;

EE_RAM_t __attribute__((section (".eeprom"))) eeprom =
{ 255, // Max PWM
		0, // Acceleration
		20, // Kp
		0, // Ki
		2, // Kd
		5, 10, 5, 5, 5, 10, // acquiering,tracking,SharpTurn,lineBreak,lineCrossing,acuteTurn;
		1000, 350, // lineCrossingHistLimit,lineBreakHistLimit;
		2, // num_to_average
		0, // loop_time
		0, //acquiering_th,
		255, //tracking_th,
		550, //sharpTurn_th,
		50, //lineBreak_th,
		1100, //lineCrossing_th;
		1, 2, 3, 675, //led_coeff[4];
		20, //delta_t for dt measurements
		2000, 2000, //turn_speed
		180, 180, //turn_error_threshod
		0, //turn_delay
		250, // trig_reset
		0, // mode
		};

EE_RAM_t eeRam;

//----- House keeping and hardware initialization -----------------------------
int main(void)
{
	mode = MODE_LINE_RACER;
	uartInit(); //Initialize UART
	uartSetBaudRate(115200);//Default Baudrate
	rprintfInit(uartSendByte);
	cmdlineInit();
	cmdlineSetOutputFunc(uartSendByte);
	vt100Init();
	vt100ClearScreen();

	a2dInit();
	// initialize the timer system
	timerInit();
	timer0SetPrescaler(TIMER_CLK_DIV64);
	timer1SetPrescaler(TIMER_CLK_DIV1);
	// initialize vt100 terminal

	// Configure Timer1 timer for PWM
	//timer1PWMInitICR(MAX_PWM_COUNT);
	//timer1PWMAOn();
	//timer1PWMBOn();
	//timer1PWMASet(0);
	//timer1PWMBSet(0);
	//timerPause(100);


	// set none-inverting mode
	TCCR0A |= (1 << COM0A1);

	TCCR0A |= (1 << COM0B1);

	// set fast PWM Mode
	TCCR0A |= (1 << WGM01) | (1 << WGM00);

	// set prescaler to 8 and starts PWM
	TCCR0B |= (1 << CS01);

	// Configure Pin Direction
	DDRB = _BV(MOTB_DIR1);
	DDRD = _BV(MOTB_DIR2) | _BV(MOTA_PWM) | _BV(MOTB_PWM) | _BV(IR_CTRL)
			| _BV(MOTA_DIR1) | _BV(MOTA_DIR2);

	//Initialize ErrorBuffer
	ErrorBufferInit(&ErrorBuffer, error_vector, ERROR_BUFFER_SIZE);

	// **********************************************************
	// PORT C
	// configure a2d port (PORTC) as input
	// so we can receive analog signals
	DDRC = 0x00;

	// make sure pull-up resistors are turned off
	PORTC = 0x00;

	// set the a2d prescaler (clock division ratio)
	// - a lower prescale setting will make the a2d converter go faster
	// - a higher setting will make it go slower but the measurements
	//   will be more accurate
	// - other allowed prescale values can be found in a2d.h
	a2dSetPrescaler(ADC_PRESCALE_DIV8);

	// set the a2d reference
	// - the reference is the voltage against which a2d measurements are made
	// - other allowed reference values can be found in a2d.h
	a2dSetReference(ADC_REFERENCE_AVCC);

	// use a2dConvert8bit(channel#) to get an 8bit a2d reading
	// use a2dConvert10bit(channel#) to get a 10bit a2d reading

	InitializeVariables();

	uint8_t i;
	for (i = 0; i < NUM_OF_SENSORS; i++)
	{
		Led.max[i] = 200;
		Led.min[i] = 15;
	}

	// start command line
	debug = 0;
	RumCmdLine();

	return 0;
}

//----- Buffer functions and operations----------------------------------------
void ErrorBufferInit(int16Buffer_t *buffer, int16_t *start, uint16_t size)
{
	// begin critical section
	CRITICAL_SECTION_START;
	// set start pointer of the buffer
	buffer->dataptr = start;
	buffer->size = size;
	// initialize index and length
	buffer->datalength = 0;
	buffer->dataindex = 0;
	// end critical section
	CRITICAL_SECTION_END;
}

unsigned char ErrorBufferAddToEnd(int16Buffer_t* buffer, int16_t data)
{
	// begin critical section
	CRITICAL_SECTION_START;
	// make sure the buffer has room
	// save data byte at end of buffer
	buffer->dataptr[buffer->dataindex] = data;
	buffer->dataindex++;
	if (buffer->dataindex >= buffer->size)
		buffer->dataindex = 0;
	// end critical section
	CRITICAL_SECTION_END;
	// return success
	return 0;
}

int16_t ErrorBufferGetValues(int16Buffer_t* buffer, uint16_t index)
{
	if (index >= buffer->size - 1)
		index = buffer->size - 1;

	if (index < buffer->dataindex)
		index = buffer->dataindex - index - 1;
	else
		index = buffer->size - index + buffer->dataindex - 1;
	CRITICAL_SECTION_START;
	int16_t data = buffer->dataptr[index];// return character at index in buffer
	CRITICAL_SECTION_END; // end critical section
	//rprintf("Index: %d = %d.\r\n",index,data);
	return data;
}
void ErrorBufferPrint(int16Buffer_t *buffer)
{
	uint8_t i;
	for (i = 0; i < buffer->size; i++)
		rprintfNum(10, 6, FALSE, ' ', buffer->dataptr[i]);
	rprintfCRLF();
}

void RumCmdLine(void)
{
	u08 c;

	// print welcome message
	vt100ClearScreen();
	vt100SetCursorPos(1, 0);
	idn();

	// initialize cmdline system
	cmdlineInit();

	// direct cmdline output to uart (serial port)
	cmdlineSetOutputFunc(uartSendByte);

	// add commands to the command database
	cmdlineAddCommand("idn", idn);
	cmdlineAddCommand("mon", RunMonitor);
	cmdlineAddCommand("test", test);
	cmdlineAddCommand("run", run);
	cmdlineAddCommand("cal", SensorCalibration);
	cmdlineAddCommand("man", manual);
	cmdlineAddCommand("?", GetVar);
	cmdlineAddCommand("set", SetVar);
	cmdlineAddCommand("reset", ResetVariables);
	cmdlineAddCommand("pwm", ManualPwm);
	cmdlineAddCommand("adc", GetSensorReading);
	cmdlineAddCommand("loop", TestLoop);

	// send a CR to cmdline input to stimulate a prompt
	cmdlineInputFunc('\r');

	// main loop handle command line input
	while (1)
	{
		// pass characters received on the uart (serial port)
		// into the cmdline processor
		while (uartReceiveByte(&c))
			cmdlineInputFunc(c);
		// run the cmdline execution functions
		cmdlineMainLoop();
		//		if (PINC & 16) {
		//			eeRam.max_pwm = 2000;
		//			BlinkStatusLed(5, 500);
		//			run();
		//		} else if (PINC & 32) {
		//			eeRam.max_pwm = 3500;
		//			BlinkStatusLed(10, 250);
		//			run();
		//		} else if (PINC & 64) {
		//			eeRam.max_pwm = 5000;
		//			BlinkStatusLed(10, 150);
		//			run();
		//		}
	}
	rprintfCRLF();
	rprintfProgStrM("Exited program.  Press reset to restart.\r\n");
}

void BlinkStatusLed(uint8_t num_of_times, uint16_t delay)
{
	uint8_t i;
	for (i = 0; i < num_of_times; i++)
	{
		toggle(PORTC,STATUS_LED);
		timerPause(delay);
		toggle(PORTC,STATUS_LED);
		timerPause(delay);
	}
}
void idn(void) ///Prints version information
{
	//Responds to a standard instrumentation *IDN? query
	rprintfProgStrM("LineORE Line Follower Version: ");
	rprintfProgStrM(FULLVERSION_STRING);
	rprintfCRLF();
}

void InitializeVariables(void) ///Initialize motor drive PID variables
{
	eeprom_read_block(&eeRam, &eeprom, sizeof(eeprom));
}

void ResetVariables(void)
{
	timer0ClearOverflowCount();

	State.state = 0;
	State.previousState = 0;
	State.currentState = 0;
	State.trigger = 0;
	State.histIn = 0;
	State.histOut = 0;
	State.histDistance = 0;
	State.dir[R] = 0;
	State.dir[L] = 0;
	State.pwm[R] = 0;
	State.pwm[L] = 0;
	/*
	 uint8_t i;
	 for (i = 0;i <NUM_OF_SENSORS;i++)
	 {
	 Led.max[i] = 100;
	 Led.min[i] = 50;
	 }
	 */
}

void GetSensorReading(void)
{
	ReadSensors();
	uint8_t mode = cmdlineGetArgInt(1);

	if(mode)
	{
		uartSendByte(0);
		uartSendByte(255);
		for (int j = 0; j < NUM_OF_SENSORS; j++)
			uartSendByte(Led.val[j]);
	}
	else
	{
		rprintf("<a>[");
		for (int j = 0; j < NUM_OF_SENSORS; j++)
		{
			rprintf("%d",Led.val[j]);
			if(j != (NUM_OF_SENSORS-1))
				rprintf(",");
		}
		rprintf("]</a>");
		rprintfCRLF();
	}
}
void SensorCalibration(void) ///Used to set MAX and MIN values used for reading normalization
{
	uint16_t i = 0, j = 0;
	uint16_t num_of_pts = cmdlineGetArgInt(2);
	if (!num_of_pts)
		num_of_pts = 200;

	for (i = 0; i < ERROR_BUFFER_SIZE; i++)
		GetPositionError();

	sbi(PORTC,STATUS_LED);
	sbi(PORTD,LED_IR);
	rprintfProgStrM("<cal>\n");
	for (i = 0; i < num_of_pts; i++)
	{
		ReadSensors();
		//GetState();
		GetPositionError();

		for (j = 0; j < NUM_OF_SENSORS; j++)
			rprintf("%d ",Led.val[j]);
		for (j = 0; j < NUM_OF_SENSORS; j++)
			rprintf("%d ",Led.norm[j]);
		rprintf("%d ",Pid.error);
		rprintf("%d ",GetMeanError(eeRam.num_to_average));
		timerPause(cmdlineGetArgInt(1));
		rprintf("\n");
	}
	cbi(PORTC,STATUS_LED);
	cbi(PORTD,LED_IR);
	rprintfProgStrM("</cal>\n");
	rprintfCRLF();
}

void TestLoop(void) ///Display current sensor readings and robot state
{
	char c;
	uint8_t pwm_1, pwm_2, top_speed=eeRam.max_pwm;
	uint8_t negative = 0;
	int16_t error = 0;
	timer0ClearOverflowCount();
	clc();
	vt100SetCursorMode(0);

	// Add <json> tags around debug data to enable easy parsing using python json module
	if (debug)
		rprintfProgStrM("<json>[");

	while (1)
	{
		uartReceiveByte(&c); //Monitor characters in the Rx buffer

		toggle(PORTB,PB5);   //Status pin flip, can also be used to measure loop time with a scope

		ReadSensors();  //Handles ADC readings and CM calculations

		if (Led.sum > eeRam.lineBreak_th)
		{
			error = (abs(Led.cm) * eeRam.k[P]) / 10;
			if (error > top_speed)
				error = top_speed;

			if (Led.cm > 0)
			{
				negative = 0;
				pwm_1 = top_speed - (uint8_t) (error);
				pwm_2 = top_speed;
			}
			else
			{
				negative = 1;
				pwm_1 = top_speed;
				pwm_2 = top_speed - (uint8_t) (error);
			}
		}
		else
		{
			pwm_1 = pwm_2 = 0;
		}
		SetPwm(1, 0, pwm_1);
		SetPwm(2, 0, pwm_2);


		timerPause(eeRam.loop_delay);

		if(debug)
		{
			if(negative)
				error *= -1;
			PrintDebug(debug, error);
			rprintfProgStrM(",");
		}
		//rprintfNum(10, 6, TRUE, ' ', Led.cm);
		//rprintfCRLF();
		if (c == 'q')
			break;
	}

	if(debug)
	{
		PrintDebug(debug, error);
		rprintfProgStrM("]</json>");
	}
	SetPwm(0, 0, 0);
	vt100SetAttr(0);
	vt100SetCursorMode(1);
}

void RunMonitor(void) ///Display current sensor readings and robot state
{
	char c;
	timer0ClearOverflowCount();
	clc();
	vt100SetCursorMode(0);
	while (1)
	{
		uartReceiveByte(&c);
		PrintMonitorPage();

		if (c == 'q')
			break;
		else if (c == 'l')
		{
			toggle(PORTD,LED_IR);
			c = ' ';
		}
		else
			;
		timerPause(100);
	}
	vt100SetAttr(0);
	vt100SetCursorMode(1);
}

void PrintMonitorPage(void)
{
	ReadSensors();
	GetState();
	GetPositionError();

	vt100SetCursorPos(2, 1);
	vt100SetAttr(VT100_BOLD);
	rprintfProgStrM("TIMER 0   : ");
	rprintfNum(10, 8, TRUE, ' ', timer0GetOverflowCount());
	rprintfCRLF();

	rprintfProgStrM("RAW ADC   : ");
	printArray(NUM_OF_SENSORS, Led.val);

	rprintfProgStrM("NORM ADC  : ");
	printArray(NUM_OF_SENSORS, Led.norm);

	rprintfProgStrM("MAX ADC   : ");
	printArray(NUM_OF_SENSORS, Led.max);

	rprintfProgStrM("MIN ADC   : ");
	printArray(NUM_OF_SENSORS, Led.min);

	rprintfProgStrM("SUM       : ");
	rprintfNum(10, 6, TRUE,  ' ', Led.cm);
	rprintfNum(10, 6, FALSE, ' ', Led.sum);
	rprintfNum(10, 6, FALSE, ' ', Led.sumR);
	rprintfNum(10, 6, FALSE, ' ', Led.sumL);

	rprintfCRLF();

	rprintfProgStrM("ERROR     : ");
	rprintfNum(10, 6, TRUE, ' ', Pid.error);
	rprintfNum(10, 6, TRUE, ' ', Pid.integral);
	rprintfNum(10, 6, TRUE, ' ', Pid.diff);
	rprintfNum(10, 10, TRUE, ' ', Pid.error_back);
	rprintfCRLF();

	rprintfProgStrM("STATE     : ");
	switch (State.state)
	{
	case ACQUIERING:
		vt100SetAttr(VT100_GREEN);
		//ProcessHysteresis(Hist.tracking);
		rprintfProgStrM("ACQUIERING");
		break;
	case TRACKING:
		vt100SetAttr(VT100_GREEN);
		//ProcessHysteresis(Hist.tracking);
		rprintfProgStrM("TRACKING");
		break;
	case ALL_STOP:
		vt100SetAttr(VT100_RED);
		//ProcessHysteresis(0);
		rprintfProgStrM("ALL_STOP");
		break;
	case LINE_CROSSING:
		vt100SetAttr(VT100_YELLOW);
		//ProcessHysteresis(Hist.lineCrossing);
		rprintfProgStrM("LINE_CROSSING");
		break;
	case LINE_BREAK:
		vt100SetAttr(VT100_MAGENTA);
		//ProcessHysteresis(Hist.lineBreak);
		rprintfProgStrM("LINE_BREAK");
		break;
	case SHARP_TURN_L:
		vt100SetAttr(VT100_CYAN);
		//ProcessHysteresis(Hist.SharpTurn);
		rprintfProgStrM("SHARP_TURN_L");
		break;
	case SHARP_TURN_R:
		vt100SetAttr(VT100_CYAN);
		//ProcessHysteresis(Hist.SharpTurn);
		rprintfProgStrM("SHARP_TURN_R");
		break;
	case ACUTE_TURN_L:
		vt100SetAttr(VT100_CYAN);
		//ProcessHysteresis(Hist.SharpTurn);
		rprintfProgStrM("ACUTE_TURN_L");
		break;
	case ACUTE_TURN_R:
		vt100SetAttr(VT100_CYAN);
		//ProcessHysteresis(Hist.SharpTurn);
		rprintfProgStrM("ACUTE_TURN_R");
		break;
	default:
		vt100SetAttr(VT100_RED);
		//ProcessHysteresis(0);
		rprintfProgStrM("UNDEFINED");
		break;
	}

	vt100SetAttr(0);
	vt100SetAttr(VT100_BOLD);
	rprintfNum(10, 5, FALSE, ' ', State.currentState);
	rprintfNum(10, 6, FALSE, ' ', State.state);
	rprintfNum(10, 6, FALSE, ' ', State.histIn);
	rprintfCRLF();

	//rprintf("ENCODERS  : ");
	//rprintfNum(10, 6, TRUE, ' ',Enc.encoder[0]);
	//rprintfNum(10, 6, TRUE, ' ',Enc.encoder[1]);
	//rprintfCRLF();rprintfCRLF();

	rprintfProgStrM("PWM  INC  : ");
	rprintfNum(10, 6, TRUE, ' ', Pid.pwm_inc);
	rprintfCRLF();

	State.pwm[L] = (uint16_t) (eeRam.max_pwm - Pid.pwm_inc);
	State.pwm[R] = (uint16_t) (eeRam.max_pwm + Pid.pwm_inc);

	rprintfProgStrM("PWM  1 2  : ");
	rprintfNum(10, 6, TRUE, ' ', State.pwm[L]);
	rprintfNum(10, 6, TRUE, ' ', State.pwm[R]);
	rprintfCRLF();

	if (debug == 1)
	{
		SetPwm(L, State.dir[L], State.pwm[L]);
		SetPwm(R, State.dir[R], State.pwm[R]);
		rprintfProgStrM("OCR1 A B  : ");
		rprintfNum(10, 6, FALSE, ' ', OCR1A);
		rprintfNum(10, 6, FALSE, ' ', OCR1B);
		rprintfCRLF();
	}
	vt100SetAttr(0);
}

void GetVar(void)
{
	clc();
	rprintfProgStrM("<var>\n");
	vt100SetAttr(VT100_BOLD);
	vt100SetAttr(VT100_GREEN);
	if (eeRam.mode)
	{
		rprintfProgStrM("  : MODE_LINE_FOLLOWER");
		rprintfCRLF();
	}
	else
	{
		rprintfProgStrM("  : MODE_LINE_RACER");
		rprintfCRLF();
	}
	rprintfProgStrM("  : PID");
	rprintfCRLF();
	vt100SetAttr(0);
	vt100SetAttr(VT100_BOLD);
	rprintfProgStrM(" 1: PWM MAX        ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.max_pwm);
	rprintfCRLF();
	rprintfProgStrM(" 2: GAIN k         ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.k[P]);
	rprintfNum(10, 6, FALSE, ' ', eeRam.k[I]);
	rprintfNum(10, 6, FALSE, ' ', eeRam.k[D]);
	rprintfCRLF();
	rprintfProgStrM(" 3: ACCEL          ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.acceleration);
	rprintfCRLF();

	vt100SetAttr(VT100_GREEN);
	rprintfProgStrM("  : HIST");
	rprintfCRLF();
	vt100SetAttr(0);
	vt100SetAttr(VT100_BOLD);
	rprintfProgStrM(" 5: ACQUIERING     ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.acquiering);
	rprintfCRLF();
	rprintfProgStrM(" 6: TRACKING       ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.tracking);
	rprintfCRLF();
	rprintfProgStrM(" 7: SHARP_TURN     ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.sharp_turn);
	rprintfCRLF();
	rprintfProgStrM(" 8: LINE_BREAK     ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.lineBreak);
	rprintfCRLF();
	rprintfProgStrM(" 9: LINE_CROSSING  ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.lineCrossing);
	rprintfCRLF();
	vt100SetAttr(VT100_GREEN);
	rprintfProgStrM("  : HIST LIMITS");
	rprintfCRLF();
	vt100SetAttr(0);
	vt100SetAttr(VT100_BOLD);
	rprintfProgStrM("10: LINE_BREAK     ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.lineBreakHistLimit);
	rprintfCRLF();
	rprintfProgStrM("11: LINE_CROSSING  ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.lineCrossingHistLimit);
	rprintfCRLF();
	vt100SetAttr(VT100_GREEN);

	rprintfProgStrM("  : GENERAL");
	rprintfCRLF();
	vt100SetAttr(0);
	vt100SetAttr(VT100_BOLD);

	rprintfProgStrM("12: num_to_average ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.num_to_average);
	rprintfCRLF();
	rprintfProgStrM("13: loop_delay     ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.loop_delay);
	rprintfCRLF();
	rprintfProgStrM("14: sharpTurn_th   ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.sharpTurn_th);
	rprintfCRLF();
	rprintfProgStrM("15: lineBreak_th   ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.lineBreak_th);
	rprintfCRLF();
	rprintfProgStrM("16: lineCrossing_th");
	rprintfNum(10, 6, FALSE, ' ', eeRam.lineCrossing_th);
	rprintfCRLF();
	rprintfProgStrM("17: debug          ");
	rprintfNum(10, 6, FALSE, ' ', debug);
	rprintfCRLF();
	rprintfProgStrM("18: led_coeff      ");
	printArray(4, eeRam.led_coeff);
	rprintfProgStrM("19: delta t        ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.delta_t);
	rprintfCRLF();
	rprintfProgStrM("20: turn_speed     ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.turn_speed[0]);
	rprintfNum(10, 6, FALSE, ' ', eeRam.turn_speed[1]);
	rprintfCRLF();
	rprintfProgStrM("21: turn_error_thr ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.turn_error_threshod[0]);
	rprintfNum(10, 6, FALSE, ' ', eeRam.turn_error_threshod[1]);
	rprintfCRLF();
	rprintfProgStrM("22: turn_delay     ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.turn_delay);
	rprintfCRLF();
	rprintfProgStrM("23: trig_reset     ");
	rprintfNum(10, 6, FALSE, ' ', eeRam.trig_reset);
	rprintfCRLF();
	vt100SetAttr(0);
	rprintfProgStrM("</var>\n");
}

void SetVar(void)
{
	uint8_t selection = cmdlineGetArgInt(1);
	uint8_t i;
	cli();
	switch (selection)
	{
	case 0:
		eeRam.mode = (uint8_t) cmdlineGetArgInt(2);
		//eeRam.maxPwm = Pid.maxPwm;
		eeprom_write_byte(&eeprom.mode, eeRam.mode);
		break;
	case 1:
		eeRam.max_pwm = (uint16_t) cmdlineGetArgInt(2);
		//eeRam.maxPwm = Pid.maxPwm;
		eeprom_write_word(&eeprom.max_pwm, eeRam.max_pwm);
		break;
	case 2:
		eeRam.k[0] = (eeRam.k[P] = (int16_t) cmdlineGetArgInt(2));
		eeprom_write_word(&eeprom.k[0], eeRam.k[0]);
		eeRam.k[1] = (eeRam.k[I] = (int16_t) cmdlineGetArgInt(3));
		eeprom_write_word(&eeprom.k[1], eeRam.k[1]);
		eeRam.k[2] = (eeRam.k[D] = (int16_t) cmdlineGetArgInt(4));
		eeprom_write_word(&eeprom.k[2], eeRam.k[2]);
		break;
	case 3:
		eeRam.acceleration = (eeRam.acceleration = cmdlineGetArgInt(2));
		eeprom_write_word(&eeprom.acceleration, eeRam.acceleration);
		break;
	case 5:
		eeRam.acquiering = cmdlineGetArgInt(2);
		eeprom_write_byte(&eeprom.acquiering, eeRam.acquiering);
		break;
	case 7:
		eeRam.sharp_turn = cmdlineGetArgInt(2);
		eeprom_write_byte(&eeprom.sharp_turn, eeRam.sharp_turn);
		break;
	case 8:
		eeRam.lineBreak = (eeRam.lineBreak = cmdlineGetArgInt(2));
		eeprom_write_byte(&eeprom.lineBreak, eeRam.lineBreak);
		break;
	case 9:
		eeRam.lineCrossing = (eeRam.lineCrossing = cmdlineGetArgInt(2));
		eeprom_write_byte(&eeprom.lineCrossing, eeRam.lineCrossing);
		break;
	case 10:
		eeRam.lineBreakHistLimit = (uint16_t) cmdlineGetArgInt(2);
		eeprom_write_word(&eeprom.lineBreakHistLimit, eeRam.lineBreakHistLimit);
		break;
	case 11:
		eeRam.lineCrossingHistLimit = (eeRam.lineCrossingHistLimit
				= cmdlineGetArgInt(2));
		eeprom_write_word(&eeprom.lineCrossingHistLimit,
				eeRam.lineCrossingHistLimit);
		break;
	case 12:
		eeRam.num_to_average = (uint8_t) cmdlineGetArgInt(2);
		eeprom_write_byte(&eeprom.num_to_average, eeRam.num_to_average);
		break;
	case 13:
		eeRam.loop_delay = (uint8_t) cmdlineGetArgInt(2);
		eeprom_write_byte(&eeprom.loop_delay, eeRam.loop_delay);
		break;
	case 14:
		eeRam.sharpTurn_th = (uint16_t) cmdlineGetArgInt(2);
		eeprom_write_word(&eeprom.sharpTurn_th, eeRam.sharpTurn_th);
		break;
	case 15:
		eeRam.lineBreak_th = (uint16_t) cmdlineGetArgInt(2);
		eeprom_write_word(&eeprom.lineBreak_th, eeRam.lineBreak_th);
		break;
	case 16:
		eeRam.lineCrossing_th = (uint16_t) cmdlineGetArgInt(2);
		eeprom_write_word(&eeprom.lineCrossing_th, eeRam.lineCrossing_th);
		break;
	case 17:
		debug = (uint8_t) cmdlineGetArgInt(2);
		break;
	case 18:
		for (i = 0; i < 4; i++)
		{
			eeRam.led_coeff[i] = (int16_t) cmdlineGetArgInt(i + 2);
			eeprom_write_word(&eeprom.led_coeff[i], eeRam.led_coeff[i]);
		}
		break;
	case 19:
		eeRam.delta_t = (uint8_t) cmdlineGetArgInt(2);
		eeprom_write_byte(&eeprom.delta_t, eeRam.delta_t);
	case 20:
		eeRam.turn_speed[0] = (uint16_t) cmdlineGetArgInt(2);
		eeRam.turn_speed[1] = (uint16_t) cmdlineGetArgInt(3);
		eeprom_write_word(&eeprom.turn_speed[0], eeRam.turn_speed[0]);
		eeprom_write_word(&eeprom.turn_speed[1], eeRam.turn_speed[1]);
		break;
	case 21:
		eeRam.turn_error_threshod[0] = (uint16_t) cmdlineGetArgInt(2);
		eeRam.turn_error_threshod[1] = (uint16_t) cmdlineGetArgInt(3);
		eeprom_write_word(&eeprom.turn_error_threshod[0],
				eeRam.turn_error_threshod[0]);
		eeprom_write_word(&eeprom.turn_error_threshod[1],
				eeRam.turn_error_threshod[1]);
		break;
	case 22:
		eeRam.turn_delay = (uint16_t) cmdlineGetArgInt(2);
		eeprom_write_word(&eeprom.turn_delay, eeRam.turn_delay);
		break;
	case 23:
		eeRam.trig_reset = (uint16_t) cmdlineGetArgInt(2);
		eeprom_write_word(&eeprom.trig_reset, eeRam.trig_reset);
		break;
	default:
		vt100SetAttr(VT100_RED);
		rprintfProgStrM("Invalid Option.");
		vt100SetAttr(0);
		rprintfCRLF();
		break;
	}
	sei();
	GetVar();
}

void printArray(uint8_t size, uint16_t *array)
{
	uint8_t i;
	for (i = 0; i < size; i++)
		rprintfNum(10, 6, FALSE, ' ', array[i]);
	rprintfCRLF();
}

void ManualPwm(void)
{

	//Manually sets the PWM value
	uint8_t motor = (uint8_t) cmdlineGetArgInt(1);
	uint8_t dir = (uint8_t) cmdlineGetArgInt(2);
	uint16_t pwm = (uint16_t) cmdlineGetArgInt(3);
	rprintf("Set PWM (motor, dir, pwm): %d, %d, %d\n", motor, dir, pwm);
	//SetPwm(motor, dir, pwm); ///Set specified motor direction and speed
	switch (motor)
	{
	case 1:
		rprintf("Motor 1: ");
		if (dir == FORWARD)
		{
			rprintf("FORWARD \n");
			cbi(PORTD,MOTA_DIR1);
			sbi(PORTD,MOTA_DIR2);
		}
		else if (dir == REVERSE)
		{
			rprintf("REVERSE \n");
			sbi(PORTD,MOTA_DIR1);
			cbi(PORTD,MOTA_DIR2);
		}
		else
		{
			cbi(PORTD,MOTA_DIR1);
			cbi(PORTD,MOTA_DIR2);
			pwm = 0;
		}
		OCR0A = pwm;
		break;
	case 2:
		rprintf("Motor 2: ");
		if (dir == FORWARD)
		{
			rprintf("FORWARD \n");
			cbi(PORTB,MOTB_DIR1);
			sbi(PORTD,MOTB_DIR2);
		}
		else if (dir == REVERSE)
		{
			rprintf("REVERSE \n");
			sbi(PORTB,MOTB_DIR1);
			cbi(PORTD,MOTB_DIR2);
		}
		else
		{
			cbi(PORTB,MOTB_DIR1);
			cbi(PORTD,MOTB_DIR2);
			pwm = 0;
		}
		OCR0B = pwm;
		break;
	default:
		OCR0A = 0;
		OCR0B = 0;
	}

}

void AllStop(void)
{
	//Stop all motors and turn off the timers
	SetPwm(0, 1, eeRam.max_pwm);
	SetPwm(1, 1, eeRam.max_pwm);
	timerPause(10);
	SetPwm(0, 0, 1);
	SetPwm(1, 0, 0);
}

void SetPwm(uint8_t motor, uint8_t dir, uint16_t pwm) ///Set specified motor direction and speed
{
	// Truth table for driving TB6552FN/FNG/FL/FLG
	// Taken from data sheet page 3
	// INPUTS           OUTPUTS MODE
	//IN1 IN2 STBY PWM  O1 O2
	//L   H   H    H    L  H    CW
	//H   L   H    H    L  H    CCW
	//L   L   H    L    L  H    STOP
	//H   H   H    H/L  L  L    Short brake
	//L   H   H    L    L  L    Short brake
	//H   L   H    L    L  H    Short brake

	//Manually sets the PWM value
//	uint8_t motor = (uint8_t) cmdlineGetArgInt(1);
//	uint8_t dir = (uint8_t) cmdlineGetArgInt(2);
//	uint16_t pwm = (uint16_t) cmdlineGetArgInt(3);
//	rprintf("Set PWM (motor, dir, pwm): %d, %d, %d\n", motor, dir, pwm);
//	//SetPwm(motor, dir, pwm); ///Set specified motor direction and speed
	switch (motor)
	{
	case 1:
		//rprintf("Motor 1: ");
		if (dir == FORWARD)
		{
			//rprintf("FORWARD \n");
			cbi(PORTD,MOTA_DIR1);
			sbi(PORTD,MOTA_DIR2);
		}
		else if (dir == REVERSE)
		{
			//rprintf("REVERSE \n");
			sbi(PORTD,MOTA_DIR1);
			cbi(PORTD,MOTA_DIR2);
		}
		else
		{
			cbi(PORTD,MOTA_DIR1);
			cbi(PORTD,MOTA_DIR2);
			pwm = 0;
		}
		OCR0A = pwm;
		break;
	case 2:
		//rprintf("Motor 2: ");
		if (dir == FORWARD)
		{
			//rprintf("FORWARD \n");
			cbi(PORTB,MOTB_DIR1);
			sbi(PORTD,MOTB_DIR2);
		}
		else if (dir == REVERSE)
		{
			//rprintf("REVERSE \n");
			sbi(PORTB,MOTB_DIR1);
			cbi(PORTD,MOTB_DIR2);
		}
		else
		{
			cbi(PORTB,MOTB_DIR1);
			cbi(PORTD,MOTB_DIR2);
			pwm = 0;
		}
		OCR0B = pwm;
		break;
	default:
		OCR0A = 0;
		OCR0B = 0;
	}
}

//----- The heart of the code - tracking and state machine implementation -----
void ReadSensors() ///Read Line sensor
{
	uint8_t j;
	int16_t sum_weighted=0, sum=0;

	for (j = 0; j < NUM_OF_SENSORS; j++) {
		Led.val[j]    = (int16_t) (a2dConvert8bit(j));
		sum_weighted += Led.val[j] * j * 8;
		sum          += Led.val[j];
	}
	sum = sum / 8;
	Led.cm = (int16_t) (sum_weighted / sum) - (int16_t)((NUM_OF_SENSORS-1) * 8 * 4);

	for (j = 0; j < NUM_OF_SENSORS; j++)
	{
		Led.min[j] = MIN(Led.val[j],Led.min[j]);
		Led.norm[j] = (Led.val[j]) - Led.min[j];
	}

	for (j = 0; j < NUM_OF_SENSORS; j++)
	{
		Led.max[j] = MAX(Led.norm[j],Led.max[j]);
		Led.norm[j] = (255 * Led.norm[j]) / Led.max[j];
		if (Led.norm[j] > 256)
			Led.norm[j] = 255;
		if (Led.norm[j] < 0)
			Led.norm[j] = 0;
	}

	Led.sumR = Led.norm[0] + Led.norm[1] + Led.norm[2];
	Led.sumL = Led.norm[3] + Led.norm[4] + Led.norm[5];
	Led.sum = Led.sumR + Led.sumL;
	Led.sumB = Led.norm[6] + Led.norm[7];
}

void GetState(void) ///Returns the line follower state
{
	//State.currentState = TRACKING;
	uint16_t innerSum = 0;
	innerSum = Led.norm[1] + Led.norm[2] + Led.norm[3] + Led.norm[4];

	if (Led.sum >= eeRam.lineCrossing_th) /// LINE_CROSSING
	{
		State.currentState = LINE_CROSSING;
		ProcessHysteresis(eeRam.lineCrossing);
		if ((State.state == State.currentState) && (State.histIn
				>= eeRam.lineCrossingHistLimit))
			State.state = ALL_STOP;
	}
	else if ((Led.sum >= eeRam.lineBreak_th)
			& (Led.sum < eeRam.lineCrossing_th)) /// TRACKING and SHARP TURNS
	{
		if ((Led.sumR > eeRam.sharpTurn_th) || (Led.sumL > eeRam.sharpTurn_th)) ///SHARP_TURN
		{
			if (Led.sumR > Led.sumL)
				State.currentState = SHARP_TURN_R;
			else
				State.currentState = SHARP_TURN_L;
			ProcessHysteresis(eeRam.sharp_turn);
		}
		else if ((Led.norm[7] > 200))
		{
			State.currentState = ACUTE_TURN_L;
			ProcessHysteresis(eeRam.acuteTurn);
		}
		else if ((Led.norm[6] > 200))
		{
			State.currentState = ACUTE_TURN_R;
			ProcessHysteresis(eeRam.acuteTurn);
		}
		else
		{
			State.currentState = TRACKING;
			ProcessHysteresis(eeRam.tracking);
			//        	if ((((Led.norm[5] > (innerSum))) | ((Led.norm[0] > (innerSum)))) &
			//        			(Led.sumB < 50))
			//        	{
			//        		State.currentState = ACQUIERING;
			//        		ProcessHysteresis(eeRam.acquiering);
			//        	}
			//        	else
			//        	{
			//        		State.currentState = TRACKING;
			//        	    ProcessHysteresis(eeRam.tracking);
			//        	}
		}
	}
	else if (Led.sum < eeRam.lineBreak_th) /// LINE BREAK
	{
		State.currentState = LINE_BREAK;
		ProcessHysteresis(eeRam.lineBreak);
		if ((State.state == State.currentState) && (State.histIn
				>= eeRam.lineBreakHistLimit))
			State.state = ALL_STOP;
	}
	else
	{
		State.currentState = State.currentState;
		State.state = State.state;
	}
}

void ProcessHysteresis(uint8_t hysteresisLimit)
{
	///===================================================================================
	if (State.currentState != State.previousState)
	{
		State.histIn = 0;
		State.histDistance = 0;
	}
	else if ((State.currentState == State.previousState) & (State.histIn
			>= hysteresisLimit))
	{
		State.histIn += 1;
		State.histDistance = 0;
		State.state = State.currentState;

		if (!State.trigger)
		{
			if (State.state == SHARP_TURN_L)
				State.trigger = SHARP_TURN_L;
			if (State.state == SHARP_TURN_R)
				State.trigger = SHARP_TURN_R;

			if (State.state == ACUTE_TURN_L)
				State.trigger = ACUTE_TURN_L;
			if (State.state == ACUTE_TURN_R)
				State.trigger = ACUTE_TURN_R;
		}

		if ((State.state == TRACKING) & (State.histIn >= eeRam.trig_reset))
			State.trigger = 0;
		if ((State.state == ACQUIERING) & (State.histIn >= 500))
			State.state = TRACKING;
	}
	else
	{
		State.histIn += 1;
		State.histDistance = 0;
	}
	State.previousState = State.currentState;
}

int16_t GetPositionError(void) ///Determine the car position
{
	uint16_t innerSum = 0;
	int16_t raw_error;
	innerSum = Led.norm[1] + Led.norm[2] + Led.norm[3] + Led.norm[4];

	if ((Led.norm[0] > Led.min[0]) & ((Led.norm[0]) > (innerSum + Led.norm[5])))
	{
		Led.norm[0] = eeRam.led_coeff[3] - Led.norm[0];
		Led.norm[1] = 0;
	}
	if ((Led.norm[5] > Led.min[5]) & ((Led.norm[5]) > (innerSum + Led.norm[0])))
	{
		Led.norm[5] = eeRam.led_coeff[3] - Led.norm[5];
		Led.norm[4] = 0;
	}
	raw_error = (eeRam.led_coeff[2] * Led.norm[0] + eeRam.led_coeff[1]
			* Led.norm[1] + eeRam.led_coeff[0] * Led.norm[2])
			- (eeRam.led_coeff[0] * Led.norm[3] + eeRam.led_coeff[1]
					* Led.norm[4] + eeRam.led_coeff[2] * Led.norm[5]);
	raw_error = raw_error / 10;

	if ((State.currentState == TRACKING) | (State.currentState == ACQUIERING))
		Pid.error = raw_error;
	else if ((State.currentState == SHARP_TURN_R) | (State.currentState
			== SHARP_TURN_L))
		Pid.error = (int16_t) Led.norm[2] - (int16_t) Led.norm[3];
	else if ((State.currentState == ACUTE_TURN_R) | (State.currentState
			== ACUTE_TURN_L))
		Pid.error = (int16_t) (Led.norm[1] + Led.norm[2])
				- (int16_t) (Led.norm[3] + Led.norm[4]);
	else
		Pid.error = 0;

	Pid.error_back = (int16_t) Led.norm[6] - (int16_t) Led.norm[7];

	Pid.diff = (ErrorBufferGetValues(&ErrorBuffer, eeRam.delta_t) - Pid.error)
			/ 4;
	Pid.integral += Pid.error;
	Pid.prevError = Pid.error;

	if (Pid.integral > 500)
		Pid.integral = 500;
	else if (Pid.integral < -500)
		Pid.integral = -500;

	ErrorBufferAddToEnd(&ErrorBuffer, Pid.error);
	//Pid.pwmInc    = (eeRam.k[P]*Pid.error) + (eeRam.k[I]*Pid.integral) + (eeRam.k[D]*Pid.diff);
	Pid.pwm_inc = (eeRam.k[P] * GetMeanError(eeRam.num_to_average))
			+ (eeRam.k[I] * Pid.integral) + (eeRam.k[D] * Pid.diff);

	if (Pid.pwm_inc > eeRam.max_pwm)
		Pid.pwm_inc = eeRam.max_pwm;
	if (Pid.pwm_inc < -1 * eeRam.max_pwm)
		Pid.pwm_inc = -1 * eeRam.max_pwm;
	return raw_error;

}
int16_t GetMeanError(uint8_t window)
{
	uint8_t i;
	int16_t sum = 0;
	for (i = 0; i < window; i++)
	{
		sum += ErrorBufferGetValues(&ErrorBuffer, i);
	}
	return sum / window;
}
void run(void)
{
	//Run the line follower loop, press q at any time to stop the robot and exit
	char c = ' ';
	uint16_t time0 = timer0GetOverflowCount();
	uint16_t loop_limit = (uint16_t) cmdlineGetArgInt(2);
	int16_t raw_error;
	uint16_t tmp_max_pwm = eeRam.max_pwm;
	uint8_t i;
	debug = (uint8_t) cmdlineGetArgInt(1);
	uint8_t num_of_right_markers = 0;

	ResetVariables();
	rprintfProgStrM("<run>");
	for (i = 0; i < ERROR_BUFFER_SIZE; i++)
	{
		ErrorBufferAddToEnd(&ErrorBuffer, 0);
		PrintDebug(debug, 0);
	}

	timer1PWMAOn();
	timer1PWMBOn();

	sbi(PORTD,LED_IR);
	if (mode == MODE_LINE_FOLLOWER)
		sbi(PORTC,STATUS_LED);
	State.state = ACQUIERING;
	timer0ClearOverflowCount();
	while (1)
	{
		time0 = timer0GetOverflowCount();
		uartReceiveByte(&c);

		ReadSensors();
		GetState();
		raw_error = GetPositionError();

		switch (State.state)
		{
		case ACQUIERING:
			eeRam.max_pwm = 2000;
			//eeRam.max_pwm = 3000;
			//eeRam.max_pwm = tmp_max_pwm;
			break;
		case TRACKING:
			///======================================================
			//TODO - could add top speed control based on the error
			eeRam.max_pwm = tmp_max_pwm;

			if ((eeRam.mode == MODE_LINE_RACER) && (State.trigger
					== ACUTE_TURN_R))
			{
				num_of_right_markers++;
				if (num_of_right_markers == 1)
				{
					sbi(PORTC,STATUS_LED);
					State.trigger = 0;
				}
				else if (num_of_right_markers == 2)
				{
					cbi(PORTC,STATUS_LED);
					SetPwm(L, State.dir[L], 5000);
					SetPwm(R, State.dir[R], 5000);
					timerPause(500);
					c = 'q';
				}
			}

			break;

		case LINE_BREAK:
			///======================================================
			if (eeRam.mode == MODE_LINE_FOLLOWER)
			{
				if (State.trigger == SHARP_TURN_L)
					SharpTurn(L);
				else if (State.trigger == SHARP_TURN_R)
					SharpTurn(R);
				else if (State.trigger == ACUTE_TURN_R)
					AcuteTurn(R);
				else if (State.trigger == ACUTE_TURN_L)
					AcuteTurn(L);
				else
					;
				//            	if (debug == 3)
				//            		c = 'q';
			}
			break;
		case ACUTE_TURN_R:
		case ACUTE_TURN_L:
			if (eeRam.mode == MODE_LINE_FOLLOWER)
				eeRam.max_pwm = 2000;
			break;
		case SHARP_TURN_R:
		case SHARP_TURN_L:
			break;
		case ALL_STOP:
			//      case LINE_CROSSING:
			///======================================================
			c = 'q';
		default:
			break;
		}

		State.dir[L] = 0;
		State.dir[R] = 0;

		State.pwm[L] = (uint16_t) (eeRam.max_pwm - Pid.pwm_inc);
		State.pwm[R] = (uint16_t) (eeRam.max_pwm + Pid.pwm_inc);

		SetPwm(L, State.dir[L], State.pwm[L]);
		SetPwm(R, State.dir[R], State.pwm[R]);

		PrintDebug(debug, raw_error);

		while ((timer0GetOverflowCount() - time0) < eeRam.loop_delay)
		{
		}

		if ((loop_limit > 0) && (loop_limit < timer0GetOverflowCount()))
			break;

		if (c == 'q')
			break;
	}
	AllStop();
	eeRam.max_pwm = tmp_max_pwm;
	rprintfProgStrM("</run>");
	rprintf("Timer0 overflow counter: %d\n",timer0GetOverflowCount());

	cbi(PORTC,STATUS_LED);
	cbi(PORTD,LED_IR);
}
void SharpTurn(uint8_t dir)
{
	uint8_t done = 0;
	uint8_t c = ' ';
	uint16_t loop_count = 0;
	uint16_t time0 = timer0GetOverflowCount();
	int16_t raw_error;

	timerPause(eeRam.turn_delay);
	eeRam.acceleration = 0;
	if (dir == R)
	{
		cbi(PORTC,DIR_L);
		cbi(PORTC,DIR_R);
		timer1PWMASet(eeRam.turn_speed[0]);
		timer1PWMBSet(eeRam.turn_speed[0]);
		State.state = EXEC_SHARP_TURN_R;
	}
	else
	{
		sbi(PORTC,DIR_L);
		sbi(PORTC,DIR_R);
		timer1PWMASet(MAX_PWM_COUNT - eeRam.turn_speed[0]);
		timer1PWMBSet(MAX_PWM_COUNT - eeRam.turn_speed[0]);
		State.state = EXEC_SHARP_TURN_L;
	}

	while (!done)
	{
		time0 = timer0GetOverflowCount();
		uartReceiveByte(&c);

		ReadSensors();
		raw_error = GetPositionError();
		PrintDebug(debug, raw_error);

		if (dir == L)
		{
			if ((raw_error > -1 * (int16_t) eeRam.turn_error_threshod[0])
					& (Led.sum > 150))
				done = 1;
		}
		else if (dir == R)
		{
			if ((raw_error < (int16_t) eeRam.turn_error_threshod[0]) & (Led.sum
					> 150))
				done = 1;
		}

		//        while ((timer0GetOverflowCount() - time0) < eeRam.loop_delay)
		//		{
		//		}
		if ((loop_count++ > 10000) | (c == 'q'))
			done = 1;

	}
	State.state = ACQUIERING;
	State.trigger = 0;
	if (loop_count++ > 10000)
		AllStop();
}
void AcuteTurn(uint8_t dir)
{
	uint8_t done = 0;
	uint8_t c = ' ';
	uint16_t loop_count = 0;
	uint16_t time0 = timer0GetOverflowCount();
	int16_t raw_error;

	timerPause(eeRam.turn_delay);

	if (dir == R)
	{
		cbi(PORTC,DIR_L);
		cbi(PORTC,DIR_R);
		timer1PWMASet(eeRam.turn_speed[1]);
		//timer1PWMASet(eeRam.turn_speed[1]/2);
		timer1PWMBSet(eeRam.turn_speed[1] - 1000);
		State.state = EXEC_ACUTE_TURN_R;
	}
	else
	{
		sbi(PORTC,DIR_L);
		sbi(PORTC,DIR_R);
		timer1PWMASet(MAX_PWM_COUNT - eeRam.turn_speed[1]);
		timer1PWMASet(MAX_PWM_COUNT - eeRam.turn_speed[1]);
		timer1PWMBSet(MAX_PWM_COUNT - eeRam.turn_speed[1] - 1000);
		State.state = EXEC_ACUTE_TURN_L;
	}
	while ((!done))
	{
		time0 = timer0GetOverflowCount();
		uartReceiveByte(&c);

		ReadSensors();
		raw_error = GetPositionError();
		PrintDebug(debug, raw_error);

		if (dir == L)
		{
			if ((raw_error > -1 * (int16_t) eeRam.turn_error_threshod[1])
					& (Led.sum > 150))
				done = 1;
		}
		else if (dir == R)
		{
			if ((raw_error < (int16_t) eeRam.turn_error_threshod[1]) & (Led.sum
					> 150))
				done = 1;
		}

		//		while ((timer0GetOverflowCount() - time0) < eeRam.loop_delay)
		//		{
		//		}

		if ((loop_count++ > 10000) | (c == 'q'))
			done = 1;
	}
	State.state = ACQUIERING;
	State.trigger = 0;
	if (loop_count++ > 10000)
		AllStop();
}

void test(void)/// Generic test function
{
	int16_t j, temp[6], cm = 0, sum = 0, sum_weight=0;
	timer0ClearOverflowCount();
//	sbi(PORTD,LED_IR);
//	if (cmdlineGetArgInt(1))
//		SharpTurn((int8_t) cmdlineGetArgInt(2));
//	else
//		AcuteTurn((int8_t) cmdlineGetArgInt(2));
//	AllStop();
//	cbi(PORTD,LED_IR);
	ReadSensors();
	for (j = 0; j < NUM_OF_SENSORS; j++)
	{
		sum_weight  += Led.val[j] * j * 8;
		sum         += Led.val[j];
	}
	sum = sum/8;
	cm   = (int16_t)(sum_weight/sum);

	rprintf("%d %d %d\n", sum_weight, sum, cm - (NUM_OF_SENSORS-1)*8*4);
}

void manual(void) ///Manual Speed control
{
}

void PrintDebug(uint8_t level, int16_t raw_error)
{
	uint8_t j;
	switch (level)
	{
	case 1:
		rprintf("%d",raw_error);
		break;
	case 2:
		rprintf("%d",Pid.error);//13
		break;
	case 3:
		rprintf("%d",Led.sum); //8
		break;
	case 4:
		rprintf("%d",State.currentState);
		break;
	case 5:
		rprintf("%d",State.state);
		break;
	case 6:
		rprintf("%d",State.trigger);
		break;
	case 9:
		for (j = 0; j < NUM_OF_SENSORS; j++)
			rprintf("%d,",Led.val[j]); //0-7
		rprintf("%d,",Led.sum); //8
		rprintf("%d,",Led.sumR); //9
		rprintf("%d,",Led.sumL); //10
		rprintf("%d,",Led.sumB); //11
		rprintf("%d,",raw_error);//12
		rprintf("%d,",Pid.error);//13
		rprintf("%d,",State.currentState);//14
		rprintf("%d,",State.state);//15
		rprintf("%d",State.trigger);
		//rprintf("\n");
		break;
	default:
		break;
	}
}
